from utime import sleep
from machine import Pin

# led灯初始化
light = Pin(25, Pin.OUT)


# 初始化上部电机
mu1 = Pin(18, Pin.OUT) # Pul-
mu2 = Pin(19, Pin.OUT) # Pul+
mu3 = Pin(20, Pin.OUT) # Dir-
mu4 = Pin(21, Pin.OUT) # Dir+

# 初始化下部电机
md1 = Pin(10, Pin.OUT)
md2 = Pin(11, Pin.OUT)
md3 = Pin(12, Pin.OUT)
md4 = Pin(13, Pin.OUT)




# 上方电机运动函数
def set_move_up(w1, w2, w3, w4):
    mu1.value(w1)
    mu2.value(w2)
    mu3.value(w3)
    mu4.value(w4)

def move_forward_up(delay, steps):
    temp = 0
    light.value(1)
    for i in range(steps):
        set_move_up(1,0,0,1)
        sleep(delay)
        set_move_up(0,1,0,1)
        sleep(delay)
        set_move_up(0,1,1,0)
        sleep(delay)
        set_move_up(1,0,1,0)
        sleep(delay)
        
        '''
        if int(i/steps*100) != temp:
            print('{}/{}'.format(int(i/steps*100), 100))
            temp = int(i/steps*100)
        '''
    stop_up()
    light.value(0)
    print('Done.')

def move_backward_up(delay, steps):
    temp = 0
    light.value(1)
    for i in range(steps):
        set_move_up(1,0,1,0)
        sleep(delay)
        set_move_up(0,1,1,0)
        sleep(delay)
        set_move_up(0,1,0,1)
        sleep(delay)
        set_move_up(1,0,0,1)
        sleep(delay)
        
        '''
        if int(i/steps*100) != temp:
            print('{}/{}'.format(int(i/steps*100), 100))
            temp = int(i/steps*100)
        '''
    stop_up()
    light.value(0)
    print('Done.')
        
def stop_up():
    set_move_up(0,0,0,0)
    
    
# 下方电机运动函数 下方电机与上方电机方向相反
def set_move_down(w1, w2, w3, w4):
    md1.value(w1)
    md2.value(w2)
    md3.value(w3)
    md4.value(w4)

def move_forward_down(delay, steps):
    temp = 0
    light.value(1)
    for i in range(steps):
        set_move_down(1,0,1,0)
        sleep(delay)
        set_move_down(0,1,1,0)
        sleep(delay)
        set_move_down(0,1,0,1)
        sleep(delay)
        set_move_down(1,0,0,1)
        sleep(delay)
        
        '''
        if int(i/steps*100) != temp:
            print('{}/{}'.format(int(i/steps*100), 100))
            temp = int(i/steps*100)
        '''
    stop_up()
    light.value(0)
    print('Done.')

def move_backward_down(delay, steps):
    temp = 0
    light.value(1)
    for i in range(steps):
        set_move_down(1,0,0,1)
        sleep(delay)
        set_move_down(0,1,0,1)
        sleep(delay)
        set_move_down(0,1,1,0)
        sleep(delay)
        set_move_down(1,0,1,0)
        sleep(delay)
        
        '''
        if int(i/steps*100) != temp:
            print('{}/{}'.format(int(i/steps*100), 100))
            temp = int(i/steps*100)
        '''
    stop_up()
    light.value(0)
    print('Done.')
        
def stop_down():
    set_move_down(0,0,0,0)


# 给角度 转
def Mot_up_forw_fast(angle):
    step = int((20000 * angle) / 360)
    move_forward_up(0.0005, step)

def Mot_up_forw_slow(angle):
    step = int((20000 * angle) / 360)
    move_forward_up(0.001, step)

def Mot_up_back_fast(angle):
    step = int((20000 * angle) / 360)
    move_backward_up(0.0005, step)
    
def Mot_up_back_slow(angle):
    step = int((20000 * angle) / 360)
    move_backward_up(0.001, step)
    
def Mot_down_forw_fast(angle):
    step = int((20000 * angle) / 360)
    move_forward_down(0.0005, step)

def Mot_down_forw_slow(angle):
    step = int((20000 * angle) / 360)
    move_forward_down(0.001, step)

def Mot_down_back_fast(angle):
    step = int((20000 * angle) / 360)
    move_backward_down(0.0005, step)
    
def Mot_down_back_slow(angle):
    step = int((20000 * angle) / 360)
    move_backward_down(0.001, step)
    
def up_fast(angle):
    if angle == 0:
        print('Done.')
    elif angle > 0:
        Mot_up_forw_fast(angle)
    else:
        Mot_up_back_fast(abs(angle))
        
def up_slow(angle):
    if angle == 0:
        print('Done.')
    elif angle > 0:
        Mot_up_forw_slow(angle)
    else:
        Mot_up_back_slow(abs(angle))
        
def down_fast(angle):
    if angle == 0:
        print('Done.')
    elif angle > 0:
        Mot_down_forw_fast(angle)
    else:
        Mot_down_back_fast(abs(angle))
        
def down_slow(angle):
    if angle == 0:
        print('Done.')
    elif angle > 0:
        Mot_down_forw_slow(angle)
    else:
        Mot_down_back_slow(abs(angle))


def helpme():
    need_print = []
    need_print.append('本程序含有如下命令')
    need_print.append('上方电机快速转动：up_fast(angle)')
    need_print.append('上方电机慢速转动：up_slow(angle)')
    need_print.append('下方电机快速转动：down_fast(angle)')
    need_print.append('下方电机慢速转动：down_slow(angle)')
    for item in need_print:
        print(item)
